#include "sensor_include.h"
#include "sensor_kalman.h"


// Constant definition

#define DEFAULT_R    ( 100 * PERCISION / 100 )
#define DEFAULT_P    (  50 * PERCISION / 100 )
#define C1A          (1000 * PERCISION / 100 )
#define C1B          (-100 * PERCISION / 100 )
#define C2A          (  12 * PERCISION / 100 )
#define C2B          ( -50 * PERCISION / 100 )

#define FILTER_INIT_DELAY  4

// Private function declaration

// Function definition

void KF_Initialize( kalman_filter *tp_filter )
{
    KF_Reset( tp_filter );
}

void KF_Reset( kalman_filter *tp_filter )
{
    tp_filter->u8_count = 0;

    tp_filter->f32_r = DEFAULT_R;
    tp_filter->f32_q = 0;

    tp_filter->f32_p[0][0] = DEFAULT_P;
    tp_filter->f32_p[0][1] = DEFAULT_P;
    tp_filter->f32_p[1][0] = DEFAULT_P;
    tp_filter->f32_p[1][1] = DEFAULT_P;

    tp_filter->f32_k[0][0] = 0;
    tp_filter->f32_k[0][1] = 0;
    tp_filter->f32_k[1][0] = 0;
    tp_filter->f32_k[1][1] = 0;

    tp_filter->f32_x[0] = 0;
    tp_filter->f32_x[1] = 0;
}


void KF_AdjustQ( kalman_filter *tp_filter, fixed32 f32_diff1, fixed32 f32_diff2 )
{
    fixed32 f32_c1, f32_c2;

    f32_c1 = F_MUL( C1A, f32_diff1 );
    f32_c1 += C1B;
    f32_c2 = Log(f32_diff1);
    f32_c2 = F_MUL( C2A, f32_c2 );
//    f32_c2 += C2B;
//    f32_c2 = F_MUL( f32_c2, f32_diff2 );
    f32_c2 = Exp(f32_c2);
    tp_filter->f32_q = F_MUL( f32_c1, f32_c2 );
}


void KF_AdjustR( kalman_filter *tp_filter, fixed32 f32_noise )
{
    fixed32 f32_temp;

    f32_temp = F_MUL( C2B, f32_noise );
    f32_temp = Exp(f32_temp);

    tp_filter->f32_r = F_MUL( DEFAULT_R, f32_temp );
}


fixed32 KF_Update ( kalman_filter *tp_filter, fixed32 f32_input0, fixed32 f32_input1 )
{
    fixed32 f32_temp;
    fixed32 f32_t00,f32_t01,f32_t10,f32_t11;

    if( tp_filter->u8_count < FILTER_INIT_DELAY)
        tp_filter->u8_count++;

    if ( tp_filter->f32_x[0] <= 0 )
        tp_filter->f32_x[0] = f32_input0;

    // P = APA'+Q
    f32_t00 = tp_filter->f32_p[0][0] + tp_filter->f32_p[0][1] + tp_filter->f32_p[1][0] + tp_filter->f32_p[1][1];
    f32_t01 = tp_filter->f32_p[0][1] + tp_filter->f32_p[1][1];
    f32_t10 = tp_filter->f32_p[1][0] + tp_filter->f32_p[1][1];
    f32_t11 = tp_filter->f32_p[1][1];
    tp_filter->f32_p[0][0] = f32_t00 + tp_filter->f32_q;
    tp_filter->f32_p[0][1] = f32_t01;
    tp_filter->f32_p[1][0] = f32_t10;
    tp_filter->f32_p[1][1] = f32_t11 + tp_filter->f32_q;

    // K = P(P+R)^-1
    f32_t00 = tp_filter->f32_p[0][0] + tp_filter->f32_r;
    f32_t01 = tp_filter->f32_p[0][1];
    f32_t10 = tp_filter->f32_p[1][0];
    f32_t11 = tp_filter->f32_p[1][1] + tp_filter->f32_r;
    f32_temp = F_MUL( f32_t00, f32_t11 ) - F_MUL( f32_t01, f32_t10 );
    if (f32_temp==0)
    {
        tp_filter->f32_k[0][0] = PERCISION;
        tp_filter->f32_k[0][1] = 0;
        tp_filter->f32_k[1][0] = 0;
        tp_filter->f32_k[1][1] = PERCISION;
    }
    else
    {
        f32_t00 =  F_DIV( f32_t00, f32_temp );
        f32_t01 = -F_DIV( f32_t01, f32_temp );
        f32_t10 = -F_DIV( f32_t10, f32_temp );
        f32_t11 =  F_DIV( f32_t11, f32_temp );
        tp_filter->f32_k[0][0] = F_MUL( tp_filter->f32_p[0][0], f32_t11 ) + F_MUL( tp_filter->f32_p[0][1], f32_t10 );
        tp_filter->f32_k[0][1] = F_MUL( tp_filter->f32_p[0][0], f32_t01 ) + F_MUL( tp_filter->f32_p[0][1], f32_t00 );
        tp_filter->f32_k[1][0] = F_MUL( tp_filter->f32_p[1][0], f32_t11 ) + F_MUL( tp_filter->f32_p[1][1], f32_t10 );
        tp_filter->f32_k[1][1] = F_MUL( tp_filter->f32_p[1][0], f32_t01 ) + F_MUL( tp_filter->f32_p[1][1], f32_t00 );
    }

    // X = AX
    tp_filter->f32_x[0] = tp_filter->f32_x[0] + tp_filter->f32_x[1];

    // X = X + K(Z-X)
    f32_t00 = f32_input0 - tp_filter->f32_x[0];
    f32_t10 = f32_input1 - tp_filter->f32_x[1];
    tp_filter->f32_x[0] += F_MUL( tp_filter->f32_k[0][0], f32_t00 ) + F_MUL( tp_filter->f32_k[0][1], f32_t10 );
    tp_filter->f32_x[1] += F_MUL( tp_filter->f32_k[1][0], f32_t00 ) + F_MUL( tp_filter->f32_k[1][1], f32_t10 );

    // P = (I-K)P
    tp_filter->f32_k[0][0] = PERCISION - tp_filter->f32_k[0][0];
    tp_filter->f32_k[0][1] = -tp_filter->f32_k[0][1];
    tp_filter->f32_k[1][0] = -tp_filter->f32_k[1][0];
    tp_filter->f32_k[1][1] = PERCISION - tp_filter->f32_k[1][1];
    f32_t00 = F_MUL( tp_filter->f32_k[0][0], tp_filter->f32_p[0][0] ) + F_MUL( tp_filter->f32_k[0][1], tp_filter->f32_p[1][0] );
    f32_t01 = F_MUL( tp_filter->f32_k[0][0], tp_filter->f32_p[0][1] ) + F_MUL( tp_filter->f32_k[0][1], tp_filter->f32_p[1][1] );
    f32_t10 = F_MUL( tp_filter->f32_k[1][0], tp_filter->f32_p[0][0] ) + F_MUL( tp_filter->f32_k[1][1], tp_filter->f32_p[1][0] );
    f32_t11 = F_MUL( tp_filter->f32_k[1][0], tp_filter->f32_p[0][1] ) + F_MUL( tp_filter->f32_k[1][1], tp_filter->f32_p[1][1] );
    tp_filter->f32_p[0][0] = f32_t00;
    tp_filter->f32_p[0][1] = f32_t01;
    tp_filter->f32_p[1][0] = f32_t10;
    tp_filter->f32_p[1][1] = f32_t11;

    //
    f32_temp = tp_filter->f32_x[0] - f32_input0;
    f32_temp = ABS(f32_temp);
    if (f32_temp > PERCISION)
    {
        KF_Reset(tp_filter);
        tp_filter->f32_x[0] = f32_input0;
    }
            
    tp_filter->f32_x[0] = CLIP( tp_filter->f32_x[0], ISIG_UPPER, ISIG_LOWER );
    tp_filter->f32_x[1] = CLIP( tp_filter->f32_x[1], ISIG_ROC_UPPER, -ISIG_ROC_UPPER );
    
    f32_temp = tp_filter->f32_x[0];

    f32_temp = WeightMean( f32_temp, f32_input0, tp_filter->u8_count * PERCISION / FILTER_INIT_DELAY );
    return f32_temp;
}
